/********************************************************************
created:	2008/10/01
created:	1:10:2008   20:03
filename: 	h:\SimoneEngine\qianye\src\qylib\math\qyMatrix4.h
author:		Xiong Jiayu
copyright:  
purpose:	
*********************************************************************/
#pragma once

#ifndef __QYMATRIX4_H__
#define __QYMATRIX4_H__

#include "qyMath.h"
#ifndef __QYMATRIX3_H__
#include "qyMatrix3.h"
#endif
#include "qyVector4.h"
#include "qyVector3.h"
#include "qyQuat.h"
#pragma warning( push )
#pragma warning( disable : 4244 )

struct Matrix3;
#define MATRIX_INVERSE_EPSILON		1e-14f
#define MATRIX_EPSILON				1e-6f

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Matrix Layout and Storage
/////////////////////////////////////////////////////////////////////////////////////////////////////////
//  
//  In our engine, matrices are represented as follows:
//  
//      +----+----+----+----+
//      | Rx | Rx | Rx | 0  |
//      |----+----+----+----|
//      | Ry | Ry | Ry | 0  |
//      |----+----+----+----|
//      | Rz | Rz | Rz | 0  |
//      |----+----+----+----|
//      | Tx | Ty | Tz |  1 |
//      +----+----+----+----+
//  
//  A "point" is a "row vector".  To transform a point using a matrix, the 
//  point is "pre-multiplied" with the matrix.  Operations performed by a 
//  sequence of matrix multiplications occur left to right.  Thus:
//  
//							  | M |     | M |     | M |
//   [x' y' z']  =  [x y z] * |op1|  *  |op2|  *  |op3|
//					          |   |     |   |     |   |
//  
//  Looking at it mathematically:
//  
//        		              |5 2 3|       x' = 5x + 7y + 8z   
//   [x' y' z']  =	[x y z] * |7 6 1|  and  y' = 2x + 6y + 4z   
//         		              |8 4 9|       z' = 3x + 1y + 9z   
//                         
//	 Matrices are PHYSICALLY stored with the following linear memory layout:
//	 
//	          0    1    2    3   
//	       +----+----+----+----+       +----+----+----+----+
//	     0 | 00 | 01 | 02 | 03 |       | Rx | Rx | Rx | 0  |      Tx = M[12]
//	       |----+----+----+----|       |----+----+----+----|      Tx = M[3][0]
//	     1 | 04 | 05 | 06 | 07 |       | Ry | Ry | Ry | 0  |
//	       |----+----+----+----|       |----+----+----+----|
//	     2 | 08 | 09 | 10 | 11 |       | Rz | Rz | Rz | 0  |
//	       |----+----+----+----|       |----+----+----+----|
//	     3 | 12 | 13 | 14 | 15 |       | Tx | Ty | Tz | 1  |
//	       +----+----+----+----+       +----+----+----+----+
//	 
//	 The data within a matrix4 is stored as 4 vector4.
//	 
//	     Vector4 mat[4]
//	 
//	 Matrix elements are accessed via mat[row][column]. 
//	 So, the "Tx" element is mat[3][0].
//	 
//	 This representation is "assignment compatible" with 4x4 matrices in both
//	 OpenGL and DirectX.  For example:
//	 
//	     matrix4 M;
//	 
//	     glMatrixMode ( GL_MODELVIEW );
//	     glLoadMatrixf( &M );
//	 
//	     SetTransform( D3DTRANSFORMSTATE_WORLD, &(D3DMATRIX(M)) );
//	  
//==============================================================================

/*
===========================================
struct Matrix4
===========================================
*/
struct Matrix4
{
public:
	Vector4				mat[4];

	Matrix4( void );
	explicit			Matrix4( const Vector4 &x, const Vector4 &y, const Vector4 &z, const Vector4 &w );
	explicit			Matrix4( const float xx, const float xy, const float xz, const float xw,
								const float yx, const float yy, const float yz, const float yw,
								const float zx, const float zy, const float zz, const float zw,
								const float wx, const float wy, const float wz, const float ww );
	explicit			Matrix4( const Matrix3 &rotation, const Vector3 &translation );
	explicit			Matrix4( const float src[ 4 ][ 4 ] );

	void				Set( const float xx, const float xy, const float xz, const float xw,
								const float yx, const float yy, const float yz, const float yw,
								const float zx, const float zy, const float zz, const float zw,
								const float wx, const float wy, const float wz, const float ww );

	const Vector4 &		operator[]( int index ) const;
	Vector4 &			operator[]( int index );
	Matrix4				operator*( const float a ) const;
	Vector4				operator*( const Vector4 &vec ) const;
	Vector3				operator*( const Vector3 &vec ) const;
	Matrix4				operator*( const Matrix4 &a ) const;
	Matrix4				operator+( const Matrix4 &a ) const;
	Matrix4				operator-( const Matrix4 &a ) const;
	Matrix4 &			operator*=( const float a );
	Matrix4 &			operator*=( const Matrix4 &a );
	Matrix4 &			operator+=( const Matrix4 &a );
	Matrix4 &			operator-=( const Matrix4 &a );

	friend Matrix4		operator*( const float a, const Matrix4 &mat );
	friend Vector4		operator*( const Vector4 &vec, const Matrix4 &mat );
	friend Vector3		operator*( const Vector3 &vec, const Matrix4 &mat );
	friend Vector4 &	operator*=( Vector4 &vec, const Matrix4 &mat );
	friend Vector3 &	operator*=( Vector3 &vec, const Matrix4 &mat );

	bool				Compare( const Matrix4 &a ) const;						// exact compare, no epsilon
	bool				Compare( const Matrix4 &a, const float epsilon ) const;	// compare with epsilon
	bool				operator==( const Matrix4 &a ) const;					// exact compare, no epsilon
	bool				operator!=( const Matrix4 &a ) const;					// exact compare, no epsilon

	void				Zero( void );
	void				Identity( void );
	bool				IsIdentity( const float epsilon ) const;
	bool				IsSymmetric( const float epsilon ) const;
	bool				IsDiagonal( const float epsilon ) const;

	void				ProjectVector( const Vector4 &src, Vector4 &dst ) const;
	void				UnprojectVector( const Vector4 &src, Vector4 &dst ) const;

	float				Trace( void ) const;
	float				Determinant( void ) const;
	Matrix4				Transpose( void ) const;	// returns transpose
	Matrix4 &			TransposeSelf( void );
	Matrix4				Inverse( void ) const;		// returns the inverse ( m * m.Inverse() = identity )
	bool				InverseSelf( void );		// returns false if determinant is zero
	Matrix4				InverseFast( void ) const;	// returns the inverse ( m * m.Inverse() = identity ) all Positive Definite matrices and P-matrices should be fine
	bool				InverseFastSelf( void );	// returns false if determinant is zero all Positive Definite matrices and P-matrices should be fine
	Matrix4				TransposeMultiply( const Matrix4 &b ) const;

	Vector3				TransformVector3( const Vector3 &vec ) const;
	Vector4				TransformVector4( const Vector4 &vec ) const;

	int					GetDimension( void ) const;
	int					GetSquareDimension( void ) const;

	const float *		ToFloatPtr( void ) const;
	float *				ToFloatPtr( void );
	const char *		ToString( int precision = 2 ) const;
	Vector3             ToVec3( void ) const;
	void                ToVec3( Vector3 &v ) const;
	void                FromVec3( const Vector3 &v );
	Matrix3             ToMat3( void ) const;
	Quat				ToQuat( void ) const;
	void                ToMat3( Matrix3 &m ) const;
	void                FromMat3( const Matrix3 &m );
	void				DecomposeSRT( Vector3 &s, Quat &r, Vector3 &t );

	//-----------------------------------------------------------------------
	// matrix utilities
	//-----------------------------------------------------------------------
	void				LookAtLH( const Vector3 &eyePos, const Vector3 &lookAt, const Vector3 &upVec );
	void				LookAtRH( const Vector3 &eyePos, const Vector3 &lookAt, const Vector3 &upVec );
	void				OrthoLH( const float width, const float height, const float zNear, const float zFar );
	void				OrthoRH( const float width, const float height, const float zNear, const float zFar );
	void				PerspectiveFovLH( const float fovy, const float aspect, const float zNear, const float zFar );
	void				PerspectiveFovRH( const float fovy, const float aspect, const float zNear, const float zFar );

	void                RotationAxis( const Vector3 &axis, const float angle );
	void                RotationAxis( const int axis, const float angle );

	void                SetScale( const Vector3 &scale );
	void                SetScale( const float x, const float y, const float z );

	Matrix4				ToUnTranslated( void ) const;
};

const Matrix4 mat4_zero( 0, 0, 0, 0,
						0, 0, 0, 0,
						0, 0, 0, 0,
						0, 0, 0, 0 );
const Matrix4 mat4_identity( 1, 0, 0, 0, 
							0, 1, 0, 0, 
							0, 0, 1, 0,
							0, 0, 0, 1 );

//-----------------------------------------------------------------------
QY_INLINE Matrix4::Matrix4( void )
{

}

//-----------------------------------------------------------------------
QY_INLINE Matrix4::Matrix4( const Vector4 &x, const Vector4 &y, const Vector4 &z, const Vector4 &w )
{
	mat[0] = x; mat[1] = y; mat[2] = z; mat[3] = w;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4::Matrix4( const float xx, const float xy, const float xz, const float xw, 
						   const float yx, const float yy, const float yz, const float yw, 
						   const float zx, const float zy, const float zz, const float zw, 
						   const float wx, const float wy, const float wz, const float ww )
{
	mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
	mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
	mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
	mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::Set( const float xx, const float xy, const float xz, const float xw, 
							const float yx, const float yy, const float yz, const float yw, 
							const float zx, const float zy, const float zz, const float zw, 
							const float wx, const float wy, const float wz, const float ww )
{
	mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
	mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
	mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
	mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4::Matrix4( const Matrix3 &rotation, const Vector3 &translation )
{
	mat[ 0 ][ 0 ] = rotation[0][0];
	mat[ 0 ][ 1 ] = rotation[1][0];
	mat[ 0 ][ 2 ] = rotation[2][0];
	mat[ 0 ][ 3 ] = translation[0];
	mat[ 1 ][ 0 ] = rotation[0][1];
	mat[ 1 ][ 1 ] = rotation[1][1];
	mat[ 1 ][ 2 ] = rotation[2][1];
	mat[ 1 ][ 3 ] = translation[1];
	mat[ 2 ][ 0 ] = rotation[0][2];
	mat[ 2 ][ 1 ] = rotation[1][2];
	mat[ 2 ][ 2 ] = rotation[2][2];
	mat[ 2 ][ 3 ] = translation[2];
	mat[ 3 ][ 0 ] = 0.0f;
	mat[ 3 ][ 1 ] = 0.0f;
	mat[ 3 ][ 2 ] = 0.0f;
	mat[ 3 ][ 3 ] = 1.0f;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4::Matrix4( const float src[4][4] )
{
	memcpy( mat, src, 4 * 4 * sizeof( float ) );
}

//-----------------------------------------------------------------------
QY_INLINE const Vector4 & Matrix4::operator[]( const int index ) const
{
	return mat[index];
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 & Matrix4::operator[]( const int index )
{
	return mat[index];
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::operator*( const float a ) const
{
	return Matrix4( 
		mat[0].x * a, mat[0].y * a, mat[0].z * a, mat[0].w * a,
		mat[1].x * a, mat[1].y * a, mat[1].z * a, mat[1].w * a,
		mat[2].x * a, mat[2].y * a, mat[2].z * a, mat[2].w * a,
		mat[3].x * a, mat[3].y * a, mat[3].z * a, mat[3].w * a );
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 Matrix4::operator*( const Vector4 &vec ) const
{
	return Vector4( 
		mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w * vec.w,
		mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w * vec.w,
		mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w * vec.w,
		mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w * vec.w );
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 Matrix4::TransformVector4( const Vector4 &vec ) const
{
	return Vector4( 
		mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z + mat[ 3 ].x * vec.w,
		mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z + mat[ 3 ].y * vec.w,
		mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z + mat[ 3 ].z * vec.w,
		mat[ 0 ].w * vec.x + mat[ 1 ].w * vec.y + mat[ 2 ].w * vec.z + mat[ 3 ].w * vec.w );
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 Matrix4::operator*( const Vector3 &vec ) const
{
	float s = mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w;
	if ( s == 0.0f ) {
		return Vector3( 0.0f, 0.0f, 0.0f );
	}
	if ( s == 1.0f ) {
		return Vector3(
			mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w,
			mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w,
			mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w );
	}
	else {
		float invS = 1.0f / s;
		return Vector3(
			(mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w) * invS,
			(mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w) * invS,
			(mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w) * invS );
	} 
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 Matrix4::TransformVector3( const Vector3 &vec ) const
{
	float s = mat[ 0 ].w * vec.x + mat[ 1 ].w * vec.y + mat[ 2 ].w * vec.z + mat[ 3 ].w;
	if ( s == 0.0f ) {
		return Vector3( 0.0f, 0.0f, 0.0f );
	}
	if ( s == 1.0f ) {
		return Vector3(
			mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z + mat[ 3 ].x,
			mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z + mat[ 3 ].y,
			mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z + mat[ 3 ].z );
	}
	else {
		float invS = 1.0f / s;
		return Vector3(
			(mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z + mat[ 3 ].x) * invS,
			(mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z + mat[ 3 ].y) * invS,
			(mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z + mat[ 3 ].z) * invS );
	} 
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::operator*( const Matrix4 &a ) const
{
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	Matrix4 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 4; i++ ) {
		for ( j = 0; j < 4; j++ ) {
			*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 4 + j ]
			+ m1Ptr[1] * m2Ptr[ 1 * 4 + j ]
			+ m1Ptr[2] * m2Ptr[ 2 * 4 + j ]
			+ m1Ptr[3] * m2Ptr[ 3 * 4 + j ];
			dstPtr++;
		}
		m1Ptr += 4;
	}
	return dst;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::operator+( const Matrix4 &a ) const
{
	return Matrix4(
		mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z, mat[0].w + a[0].w,
		mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z, mat[1].w + a[1].w,
		mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z, mat[2].w + a[2].w,
		mat[3].x + a[3].x, mat[3].y + a[3].y, mat[3].z + a[3].z, mat[3].w + a[3].w );
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::operator-( const Matrix4 &a ) const
{
	return Matrix4( 
		mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z, mat[0].w - a[0].w,
		mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z, mat[1].w - a[1].w,
		mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z, mat[2].w - a[2].w,
		mat[3].x - a[3].x, mat[3].y - a[3].y, mat[3].z - a[3].z, mat[3].w - a[3].w );
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 & Matrix4::operator*=( const float a )
{
	mat[0].x *= a; mat[0].y *= a; mat[0].z *= a; mat[0].w *= a;
	mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; mat[1].w *= a;
	mat[2].x *= a; mat[2].y *= a; mat[2].z *= a; mat[2].w *= a;
	mat[3].x *= a; mat[3].y *= a; mat[3].z *= a; mat[3].w *= a;
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 & Matrix4::operator*=( const Matrix4 &a )
{
	*this = (*this) * a;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 & Matrix4::operator+=( const Matrix4 &a )
{
	mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z; mat[0].w += a[0].w;
	mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z; mat[1].w += a[1].w;
	mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z; mat[2].w += a[2].w;
	mat[3].x += a[3].x; mat[3].y += a[3].y; mat[3].z += a[3].z; mat[3].w += a[3].w;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 & Matrix4::operator-=( const Matrix4 &a )
{
	mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z; mat[0].w -= a[0].w;
	mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z; mat[1].w -= a[1].w;
	mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z; mat[2].w -= a[2].w;
	mat[3].x -= a[3].x; mat[3].y -= a[3].y; mat[3].z -= a[3].z; mat[3].w -= a[3].w;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 operator*( const float vec, const Matrix4 &mat )
{
	return mat * vec;
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 operator*( const Vector4 &vec, const Matrix4 &mat )
{
	return mat * vec;
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 &operator*=( Vector4 &vec, const Matrix4 &mat )
{
	vec = mat * vec;
	return vec;
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 operator*( const Vector3 &vec, const Matrix4 &mat )
{
	return mat * vec;
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 &operator*=( Vector3 &vec, const Matrix4 &mat )
{
	vec = mat *vec;
	return vec;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::Transpose() const
{
	Matrix4	transpose;
	int		i, j;

	for( i = 0; i < 4; i++ ) {
		for( j = 0; j < 4; j++ ) {
			transpose[ i ][ j ] = mat[ j ][ i ];
		}
	}
	return transpose;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 &Matrix4::TransposeSelf()
{
	float	temp;
	int		i, j;

	for( i = 0; i < 4; i++ ) {
		for( j = i + 1; j < 4; j++ ) {
			temp = mat[ i ][ j ];
			mat[ i ][ j ] = mat[ j ][ i ];
			mat[ j ][ i ] = temp;
		}
	}
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::LookAtLH( const Vector3 &eyePos, const Vector3 &lookAt, const Vector3 &upVec )
{
	Vector3 zaxis = lookAt - eyePos;
	zaxis.Normalize();

	Vector3	xaxis = upVec.Cross( zaxis );
	xaxis.Normalize();
	Vector3 yaxis = zaxis.Cross( xaxis );
	this->Set(
		xaxis.x,           yaxis.x,          zaxis.x,          0,
		xaxis.y,           yaxis.y,          zaxis.y,          0,
		xaxis.z,           yaxis.z,          zaxis.z,          0,
		-xaxis * eyePos,  -yaxis * eyePos,   -zaxis * eyePos,  1
		);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::LookAtRH( const Vector3 &eyePos, const Vector3 &lookAt, const Vector3 &upVec )
{
#ifndef _GLES
	Vector3 zaxis = eyePos - lookAt;
	zaxis.Normalize();
	Vector3	xaxis = upVec.Cross( zaxis );
	xaxis.Normalize();
	Vector3	yaxis = zaxis.Cross( xaxis );
	this->Set(
		xaxis.x,           yaxis.x,           zaxis.x,          0,
		xaxis.y,           yaxis.y,           zaxis.y,          0,
		xaxis.z,           yaxis.z,           zaxis.z,          0,
		-xaxis * eyePos,  -yaxis * eyePos,	 -zaxis * eyePos,   1
		);
#else
	Vector3 zaxis = eyePos - lookAt;
	zaxis.Normalize();
	Vector3	xaxis = upVec.Cross( zaxis );
	xaxis.Normalize();
	Vector3	yaxis = zaxis.Cross( xaxis );
	yaxis.Normalize();
	this->Set(
		xaxis.x,           yaxis.x,           zaxis.x,          0,
		xaxis.y,           yaxis.y,           zaxis.y,          0,
		xaxis.z,           yaxis.z,           zaxis.z,          0,
		-xaxis * eyePos,  -yaxis * eyePos,	 -zaxis * eyePos,   1
		);
#endif
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::OrthoLH( const float width, const float height, const float zNear, const float zFar )
{
	this->Set(
		2/width,	0,			0,					0,
		0,			2/height,	0,					0,
		0,			0,			1/(zFar-zNear),		0,
		0,			0,			zNear/(zNear-zFar), 1
		);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::OrthoRH( const float width, const float height, const float zNear, const float zFar )
{
	this->Set(
		2/width,	0,			0,					0,
		0,			2/height,	0,					0,
		0,			0,			1/(zNear-zFar),		0,
		0,			0,			zNear/(zNear-zFar), 1
		);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::PerspectiveFovLH( const float fovy, const float aspect, const float zNear, const float zFar )
{
	float yScale = 1.0f / tanf(fovy * 0.5f );
	float xScale = yScale / aspect;

	this->Set( 
		xScale,     0,          0,               0,
		0,        yScale,       0,               0,
		0,          0,       zFar/(zFar-zNear),         1,
		0,          0,       -zNear*zFar/(zFar-zNear),     0
		);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::PerspectiveFovRH( const float fovy, const float aspect, const float zNear, const float zFar )
{
#ifndef _GLES
	float yScale = 1.0f / tanf(fovy * 0.5f );
	float xScale = yScale / aspect;

	this->Set( 
		xScale,     0,          0,               0,
		0,        yScale,       0,               0,
		0,          0,       zFar/(zNear-zFar),         -1,
		0,          0,       2*zNear*zFar/(zNear-zFar),     0
		);
#endif
#ifdef _GLES
	float f, n;

	// cotangent(a) == 1.0f / tan(a);
	f = 1.0f / (float)tanf(fovy * 0.5f);
	n = 1.0f / (zNear - zFar);

	this->Set(
		f / aspect, 0, 0, 0,
		0, f, 0, 0,
		0, 0, (zFar+zNear)*n, -1,
		0, 0, (2*zFar*zNear)*n, 0 );
/*
	mOut.f[ 1] = 0;
	mOut.f[ 2] = 0;
	mOut.f[ 3] = 0;

	mOut.f[ 4] = 0;
	mOut.f[ 5] = f;
	mOut.f[ 6] = 0;
	mOut.f[ 7] = 0;

	mOut.f[ 8] = 0;
	mOut.f[ 9] = 0;
	mOut.f[10] = (fFar + fNear) * n;
	mOut.f[11] = -1;

	mOut.f[12] = 0;
	mOut.f[13] = 0;
	mOut.f[14] = (2 * fFar * fNear) * n;
	mOut.f[15] = 0;
*/
#endif
/*
#ifdef _GLES
	this->Set( 
		xScale,     0,          0,               0,
		0,        yScale,       0,               0,
		0,        0,        (zFar + zNear) / (zFar - zNear), -(2 * zFar * zNear) / (zFar - zNear),
		0,        0,        1, 0);
#endif
*/
}

//-----------------------------------------------------------------------
QY_INLINE const float * Matrix4::ToFloatPtr( void ) const
{
	return &mat[0].x;
}

//-----------------------------------------------------------------------
QY_INLINE float * Matrix4::ToFloatPtr( void )
{
	return &mat[0].x;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::Identity()
{
	mat[0].Set( 1, 0, 0, 0 );
	mat[1].Set( 0, 1, 0, 0 );
	mat[2].Set( 0, 0, 1, 0 );
	mat[3].Set( 0, 0, 0, 1 );
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::Inverse() const
{
	Matrix4 invMat;

	invMat = *this;
	invMat.InverseSelf();
	return invMat;
}

//-----------------------------------------------------------------------
QY_INLINE bool Matrix4::InverseSelf()
{
	double det, invDet;

	// 2x2 sub-determinants required to calculate 4x4 determinant
	float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
	float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
	float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
	float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
	float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
	float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];

	// 3x3 sub-determinants required to calculate 4x4 determinant
	float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
	float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
	float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
	float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;

	det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );

	if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
		return false;
	}

	invDet = 1.0f / det;

	// remaining 2x2 sub-determinants
	float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
	float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
	float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
	float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
	float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
	float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];

	float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
	float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
	float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
	float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
	float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
	float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];

	// remaining 3x3 sub-determinants
	float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
	float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
	float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
	float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;

	float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
	float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
	float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
	float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;

	float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
	float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
	float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
	float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;

	mat[0][0] =	- det3_213_123 * invDet;
	mat[1][0] = + det3_213_023 * invDet;
	mat[2][0] = - det3_213_013 * invDet;
	mat[3][0] = + det3_213_012 * invDet;

	mat[0][1] = + det3_203_123 * invDet;
	mat[1][1] = - det3_203_023 * invDet;
	mat[2][1] = + det3_203_013 * invDet;
	mat[3][1] = - det3_203_012 * invDet;

	mat[0][2] = + det3_301_123 * invDet;
	mat[1][2] = - det3_301_023 * invDet;
	mat[2][2] = + det3_301_013 * invDet;
	mat[3][2] = - det3_301_012 * invDet;

	mat[0][3] = - det3_201_123 * invDet;
	mat[1][3] = + det3_201_023 * invDet;
	mat[2][3] = - det3_201_013 * invDet;
	mat[3][3] = + det3_201_012 * invDet;

	return true;
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 Matrix4::ToVec3( void ) const
{
	return Vector3( mat[3][0], mat[3][1], mat[3][2] );
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::ToVec3( Vector3 &v ) const
{
	v.Set( mat[3][0], mat[3][1], mat[3][2] );
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::FromVec3( const Vector3 &v )
{
	mat[3][0] = v[0]; mat[3][1] = v[1]; mat[3][2] = v[2];
}

//-----------------------------------------------------------------------
QY_INLINE Matrix3 Matrix4::ToMat3( void ) const
{
	return Matrix3( mat[0][0], mat[0][1], mat[0][2],
		mat[1][0], mat[1][1], mat[1][2],
		mat[2][0], mat[2][1], mat[2][2] );
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::ToMat3( Matrix3 &m ) const
{
	m[0].Set( mat[0][0], mat[0][1], mat[0][2] );
	m[1].Set( mat[1][0], mat[1][1], mat[1][2] );
	m[2].Set( mat[2][0], mat[2][1], mat[2][2] );
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::FromMat3( const Matrix3 &m )
{
	mat[0][0] = m[0][0]; mat[0][1] = m[0][1]; mat[0][2] = m[0][2];
	mat[1][0] = m[1][0]; mat[1][1] = m[1][1]; mat[1][2] = m[1][2];
	mat[2][0] = m[2][0]; mat[2][1] = m[2][1]; mat[2][2] = m[2][2];
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::RotationAxis( const Vector3 &axis, const float angle )
{
	Vector3 normal = axis;
	normal.Normalize();

	// Get sin and cosine of rotation angle

	float	s, c;
	qySinCos( angle, s, c );

	// Compute 1 - cos(theta) and some common subexpressions

	float	a = 1.0f - c;
	float	ax = a * normal.x;
	float	ay = a * normal.y;
	float	az = a * normal.z;

	// Set the matrix elements.  There is still a little more
	// opportunity for optimization due to the many common
	// subexpressions.  We'll let the compiler handle that...

	mat[0][0] = ax*normal.x + c;
	mat[0][1] = ax*normal.y + normal.z*s;
	mat[0][2] = ax*normal.z - normal.y*s;
	mat[0][3] = 0;

	mat[1][0] = ay*normal.x - normal.z*s;
	mat[1][1] = ay*normal.y + c;
	mat[1][2] = ay*normal.z + normal.x*s;
	mat[1][3] = 0;

	mat[2][0] = az*normal.x + normal.y*s;
	mat[2][1] = az*normal.y - normal.x*s;
	mat[2][2] = az*normal.z + c;
	mat[2][3] = 0;

	mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0f;
	mat[3][3] = 1.0f;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::RotationAxis( const int axis, const float angle )
{
	// Get sin and cosine of rotation angle

	float	s, c;
	qySinCos( angle, s, c );

	// Check which axis they are rotating about

	switch ( axis ) {

case 0: // Rotate about the x-axis
	mat[0][0] = 1.0f; mat[0][1] = 0.0f; mat[0][2] = 0.0f; mat[0][3] = 0.0f;
	mat[1][0] = 0.0f; mat[1][1] = c;    mat[1][2] = s; mat[1][3] = 0.0f;
	mat[2][0] = 0.0f; mat[2][1] = -s;   mat[2][2] = c; mat[2][3] = 0.0f;
	break;

case 1: // Rotate about the y-axis
	mat[0][0] = c;    mat[0][1] = 0.0f; mat[0][2] = -s; mat[0][3] = 0.0f;
	mat[1][0] = 0.0f; mat[1][1] = 1.0f; mat[1][2] = 0.0f; mat[1][3] = 0.0f;
	mat[2][0] = s;    mat[2][1] = 0.0f; mat[2][2] = c;  mat[2][3] = 0.0f;
	break;

case 2: // Rotate about the z-axis
	mat[0][0] = c;    mat[0][1] = s;    mat[0][2] = 0.0f;   mat[0][3] = 0.0f;
	mat[1][0] = -s;   mat[1][1] = c;    mat[1][2] = 0.0f;   mat[1][3] = 0.0f;
	mat[2][0] = 0.0f; mat[2][1] = 0.0f; mat[2][2] = 1.0f;   mat[2][3] = 0.0f;
	break;

default:
	// bogus axis index
	assert(false);
	}

	// Reset the translation portion

	mat[3][0] = mat[3][1] = mat[3][2] = 0.0f;
	mat[3][3] = 1.0f;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::SetScale( const float x, const float y, const float z )
{
	*this = mat4_identity;
	mat[0][0] = x;
	mat[1][1] = y;
	mat[2][2] = z;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::SetScale( const Vector3 &scale )
{
	*this = mat4_identity;
	mat[0][0] = scale[0];
	mat[1][1] = scale[1];
	mat[2][2] = scale[2];
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Matrix4::ToUnTranslated() const
{
	Matrix4 mat( *this );
	mat.mat[3].Set( 0, 0, 0, 1 );
	return mat;
}

//-----------------------------------------------------------------------
QY_INLINE Quat Matrix4::ToQuat() const
{
	Quat q;
	float		trace;
	float		s;
	float		t;
	int     	i;
	int			j;
	int			k;

	static int 	next[ 3 ] = { 1, 2, 0 };

	trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];

	if ( trace > 0.0f ) {

		t = trace + 1.0f;
		s = ( 1.0f / sqrtf( t ) ) * 0.5f;

		q.w = s * t;
		q.x = ( mat[ 1 ][ 2 ] - mat[ 2 ][ 1 ] ) * s;
		q.y = ( mat[ 2 ][ 0 ] - mat[ 0 ][ 2 ] ) * s;
		q.z = ( mat[ 0 ][ 1 ] - mat[ 1 ][ 0 ] ) * s;

	} else {

		i = 0;
		if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
			i = 1;
		}
		if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
			i = 2;
		}
		j = next[ i ];
		k = next[ j ];

		t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
		s = 1/ sqrtf( t ) * 0.5f;

		q[i] = s * t;
		q[3] = ( mat[ j ][ k ] - mat[ k ][ j ] ) * s;
		q[j] = ( mat[ i ][ j ] + mat[ j ][ i ] ) * s;
		q[k] = ( mat[ i ][ k ] + mat[ k ][ i ] ) * s;
	}
	return q;
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix4::DecomposeSRT( Vector3 &s, Quat &r, Vector3 &t )
{
	s.Set( sqrtf( mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1] +  mat[0][2]*mat[0][2] ),
		sqrtf( mat[1][0]*mat[1][0] + mat[1][1]*mat[1][1] + mat[1][2]*mat[1][2] ),
		sqrtf( mat[2][0]*mat[2][0] + mat[2][1]*mat[2][1] + mat[2][2]*mat[2][2] ) );
	r.FromMatrix4( *this );
	t.Set( mat[3][0], mat[3][1], mat[3][2] );
}

//-----------------------------------------------------------------------

#pragma warning( pop ) //  warning C4244, double to float

#endif
